/*
 * Copyright (c) 2021 Huawei Device Co., Ltd.
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "hcsr04_control.h"
#include "gpio_if.h"
#include <stdio.h>
#include <sys/time.h>

#define GPIO_INPUT_PIN_3     3          // echo
#define GPIO_OUTPUT_PIN_7    7          // trig
#define DISTANCE_NUMBER     16
#define MEASURE_INTERVAL_MS 5

#define HCSR04_TRIG_TIME    20
#define MILLION_SECONDS     1000000

#define ONE_MITER_IN_DECIMITER  100

#ifndef PIN_HIGH
#define PIN_HIGH    1
#endif

#ifndef PIN_LOW
#define PIN_LOW 0
#endif

/**
 * @brief This function is used for GPIO initialization, input/output settings, and level settings.
 */
void HCSR04_Init(void)
{
}

/**
 * @brief Deinitializes a GPIO device.
*/
void HCSR04_Deinit(void)
{
}

static unsigned long GetMicrosecond(void)
{
    struct timeval tv = {0};
    gettimeofday(&tv, NULL);

    return tv.tv_sec * MILLION_SECONDS + tv.tv_usec;
}
/**
 * @brief read hcsr04 module echo value and calculate distance
 * @return a distance by single measurement
*/
static float GetDistance(void)
{
    float distance;
    unsigned int start_time;
    unsigned int time;
    int value = PIN_LOW;
    unsigned int flag = 0;

    GpioWrite(GPIO_OUTPUT_PIN_7, PIN_HIGH);
    usleep(HCSR04_TRIG_TIME);
    GpioWrite(GPIO_OUTPUT_PIN_7, PIN_LOW);

    while (1) {
        if (GpioRead(GPIO_INPUT_PIN_3, &value) < 0) {
            printf("GpioRead failed!\n");
            sleep(1);
            continue;
        }
        if (value == PIN_HIGH && flag == 0) {
            start_time = GetMicrosecond();
            flag = 1;
        }

        if (value == PIN_LOW && flag == 1) {
            time = (unsigned int)(GetMicrosecond() - start_time);
            break;
        }
    }

    distance = time * 0.00034 / 2;      // dis = times * 0.00034 / 2, the unit is metre

    return distance;
}

/**
 * @brief  Bubble Sort,which is from small to large
*/
static void sort(float *buf, unsigned int length)
{
    int i, j;

    for (i = 0; i < length; i++) {
        for (j = 0; j < (length - i - 1); j++) {
            if (buf[j] > buf[j + 1]) {
                float tmp = buf[j];
                buf[j] = buf[j + 1];
                buf[j + 1] = tmp;
            }
        }
    }
}

/**
 * @brief median filter
 * @return distance
*/

static float FilteringData(void)
{
    float distBuff[DISTANCE_NUMBER];
    int total = DISTANCE_NUMBER;

    for (int i = 0; i < total; i++) {
        distBuff[i] = GetDistance();
        usleep(MEASURE_INTERVAL_MS * 1000);
    }

    sort(distBuff, total);

    return distBuff[(total - 1) / 2];   // 2 half of the array
}

unsigned int HCSR04_GetMeasurementDistance (void)
{
    return FilteringData() * ONE_MITER_IN_DECIMITER;
}
